/*
 * KTracker.cpp
 *
 *  Created on: May 29, 2009
 *      Author: flaviotruzzi
 */

#include "KTracker.h"
#include <math.h>


KTracker::KTracker() {
    storage = cvCreateMemStorage(0);
    xproc_cov = 100;
    xmeas_cov = 1e-3;
    yproc_cov = 100;
    ymeas_cov = 1e-3;
    xkalman = cvCreateKalman(3,1,0);
    ykalman = cvCreateKalman(3,1,0);
    A[0] = 1.0;
    A[1] = 1.0;
    A[2] = 1.0;
    A[3] = 0.0;
    A[4] = 1.0;
    A[5] = 1.0;
    A[6] = 0.0;
    A[7] = 0.0;
    A[8] = 1.0;

    // P
    cvSetIdentity( xkalman->process_noise_cov, cvRealScalar(xproc_cov) );
    cvSetIdentity( ykalman->process_noise_cov, cvRealScalar(yproc_cov) );
    // R
    cvSetIdentity( xkalman->measurement_noise_cov, cvRealScalar(xmeas_cov) );
    cvSetIdentity( ykalman->measurement_noise_cov, cvRealScalar(ymeas_cov) );
    // P
    cvSetIdentity( xkalman->error_cov_post, cvRealScalar(1));
    cvSetIdentity( ykalman->error_cov_post, cvRealScalar(1));

    memcpy(xkalman->transition_matrix->data.fl, A, sizeof(A));
    memcpy(ykalman->transition_matrix->data.fl, A, sizeof(A));

    // Randomise starting value of data
    rng = cvRNG(-1);
    cvRandArr(&rng, xkalman->state_post, CV_RAND_NORMAL, cvRealScalar(0),cvRealScalar(0.1));
    cvRandArr(&rng, ykalman->state_post, CV_RAND_NORMAL, cvRealScalar(0),cvRealScalar(0.1));
    xkalman->state_post->data.fl[0] = 0;
    ykalman->state_post->data.fl[0] = 0;

    xmeasurement = cvCreateMat(1,1,CV_32FC1);
    ymeasurement = cvCreateMat(1,1,CV_32FC1);
    raioPixel = 0.0;

}

KTracker::~KTracker() {
  cvReleaseKalman( &xkalman );
  cvReleaseKalman( &ykalman );
  cvReleaseMemStorage(&storage);
}

void KTracker::getContour(IplImage* src) {
  numContour = cvFindContours( src, storage, &contour, sizeof(CvContour),
			       CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE, cvPoint(0,0));
}
void KTracker::getOInfo() {
	for( ; contour != 0; contour = contour->h_next ) {
		// Aproxima e extrai
		if (fabs(cvContourArea(contour,CV_WHOLE_SEQ)) > 100)  {
			contourtop = cvApproxPoly( contour, sizeof(CvContour), storage, CV_POLY_APPROX_DP, 2, 0 );
			cvMoments(contourtop,&Moment,1);
			cvGetCentralMoment(&Moment,1,1);
			massCenter = cvPoint((int)(Moment.m10/Moment.m00),(int)(Moment.m01/Moment.m00));
			raioPixel = sqrt(fabs(cvContourArea(contour,CV_WHOLE_SEQ))/M_PI);
		}
    }
}
void KTracker::predict() {
	xmeasurement->data.fl[0]=(double)massCenter.x;
	ymeasurement->data.fl[0]=(double)massCenter.y;
	cvKalmanPredict(xkalman,0);
	cvKalmanPredict(ykalman,0);
}
void KTracker::correct() {
	cvKalmanCorrect(xkalman,xmeasurement);
	cvKalmanCorrect(ykalman,ymeasurement);
	massFilter = cvPoint(xkalman->state_post->data.fl[0],ykalman->state_post->data.fl[0]);
}
double KTracker::getDistance() {
	return (33409.50635857/raioPixel)- 83.84543058;
	// 33409... achado experimentalmente
	// 83...achado experimentalmente
}
int KTracker::getW() {
	if (massFilter.x > 320)
		return 1;
	else
		return -1;
}
